package kit.edu.lego.gruppe3.levels.line;

import kit.edu.lego.gruppe3.ComponentsRepository;
import kit.edu.lego.gruppe3.levels.LevelSelector;
import kit.edu.lego.gruppe3.sensors.Scanner;

import lejos.nxt.LightSensor;
//
//import lejos.nxt.NXTRegulatedMotor;
//import lejos.nxt.SensorPort;
//import lejos.nxt.TouchSensor;
//import lejos.nxt.UltrasonicSensor;
//import lejos.robotics.navigation.*;
//import lejos.robotics.pathfinding.SearchAlgorithm;
//import lejos.util.Delay;
//
//
//
//
//public class LineFollower {
//	
//	public static void followLine ()
//	{
////		
////		UltrasonicSensor sonicSensor = ComponentsRepository.getInstance().getSonicSensor();
////		LightSensor lightSensor = ComponentsRepository.getInstance().getLightSensor();
////		DifferentialPilot pilot = new DifferentialPilot(1.1f, 5.9f, Motor.C, Motor.A, false);  // parameters in inches
////		NXTRegulatedMotor rightWheel = ComponentsRepository.getInstance().getRightWheel();
////		NXTRegulatedMotor leftWheel = ComponentsRepository.getInstance().getLeftWheel();
////		lightSensor.setFloodlight(true);
////		
////		TouchSensor leftTouchS = ComponentsRepository.getInstance().getLeftTouchSensor();
////		TouchSensor rightTouchS = ComponentsRepository.getInstance().getRightTouchSensor();
////		
////		int minDistance = 13;
////		int maxDistance = 18;
////		
////		boolean distanceToClose = false;
////		boolean distanceToFar = false;
////		
////		int distance;
////		
////		while(true)
////		{
////			LCD.drawString(lightSensor.readValue() + "", 0, 3);
////			LCD.drawString(lightSensor.readNormalizedValue() + "", 0, 4);
////
////			
//////			if (leftTouchS.isPressed() || rightTouchS.isPressed()) 
//////			{
//////				
//////				pilot.setTravelSpeed(10);
//////				pilot.travel(-1.5f);
//////				pilot.rotate(110);
//////				
//////				while((sonicSensor.getDistance() <= 13) || (sonicSensor.getDistance() >= 18) && lightSensor.readValue() < 37)
//////				{
////////					distance = ComponentsRepository.getInstance().getSonicSensor().getDistance();
////////					distanceToClose = minDistance > distance;
////////					distanceToFar = maxDistance < distance;
//////					
//////					if (sonicSensor.getDistance() <= 13){
//////						pilot.setTravelSpeed(5);
////////						robot.travel(2.2f);
////////						robot.rotate(25);
//////						pilot.steer(60);
//////						
//////					}
//////					
//////					else if (sonicSensor.getDistance() >= 18){
//////						pilot.setTravelSpeed(5);
////////						robot.travel(2.2f);
////////						robot.rotate(-25);
//////						pilot.steer(-60);
//////					}
//////					
//////					else
//////					{
//////						pilot.setTravelSpeed(15);
//////						pilot.forward();
//////					}
//////				}
//////			}
////
////			
////			if (lightSensor.readValue() >= 37 && lightSensor.readValue() < 62)
////			{
////
////				pilot.setTravelSpeed(10);
////				pilot.steer(-180);
////				
////
////			}
////			else if (lightSensor.readValue() < 37)
////			{
////				
////				pilot.setTravelSpeed(5);
////				pilot.steer(110);
////				
////				
////			}
////				
////
////		}
////		

import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.subsumption.*;

public class LineFollower implements Behavior
{

	private DifferentialPilot robot = ComponentsRepository.getInstance().getRobotPilot();
	LightSensor lightSensor = ComponentsRepository.getInstance().getLightSensor();

	@Override
	public void action()
	{
		robot.setTravelSpeed(10);
		robot.travel(40, true);
		while (robot.isMoving())
			if (lightSensor.readValue() < ComponentsRepository.LIGHT_TRASHOLD
					|| Scanner.getRangeAt(0, true) < 20)
			{
				robot.stop();
				return;

			}
	}

	@Override
	public void suppress()
	{

	}

	@Override
	public boolean takeControl()
	{
		return LevelSelector.getInstance().isLine()
				&& !ComponentsRepository.evadeObstacle
				&& lightSensor.readValue() >= ComponentsRepository.LIGHT_TRASHOLD 
				&& lightSensor.readValue() < 60
				&& Scanner.getRangeAt(0, true) >= 20;
	}

}
